%% Finding Trajectories for Helicopter Tour

clear all; close all;

run cvx/cvx_setup;

% disp('Reading triangulation info');
% [sites, edges, triangles] = readTriangles('delaunay.edges');
% disp('Making triangle data structure');
% gridStructure = constructTriangleGrid(sites,edges,triangles);
load('triangles.mat');
load('gridStructure.mat');

N = 3; % subdivisions per arc for arc length approx.
eps = 0.0001; delta = 20; delta2 = 1500; safety_thresh = 0; iteration = 50; margin = 15;
checkArcs = []; fixedIndex = [];
paramDist = 0.3; paramCurve = 0.7; numAddPoints = 1; firstRun = 1;

startP = [
-3603 4592 6.1
3879 403 95.2
-882 14193 35.3
-3526 8334 30.4
4899 9416 1140.5
-3711 10251 30.6
8965 5690 330.7
-6061 10869 24.8
13835 7236 55.9
4744 -246 22.10
11238 -6553 100.11
5177 -13990 7.12
19493 2923 70.13
-5783 2536 31.14
261 959 70.15
];

% startP = optimizePath(filename)
% startP = [
% 5177.0 -13990.0 7.12
% 11238.0 -6553.0 100.11
% 4744.0 -246.0 22.1
% 3879.0 403.0 95.2
% 261.0 959.0 70.15
% -3603.0 4592.0 6.1
% -5783.0 2536.0 31.14
% -3526.0 8334.0 30.4
% -3711.0 10251.0 30.6
% -6061.0 10869.0 24.8
% -882.0 14193.0 35.3
% 4899.0 9416.0 1140.5
% 8965.0 5690.0 330.7
% 13835.0 7236.0 55.9
% 19493.0 2923.0 70.13
% ];

P = startP;

% increase number of sites by adding more inbetween
[P origIndex newIndex] = interpolateP(P, sites);

cvx_quiet(true);

for iter=1:iteration
    
    initialize_cvx;
    disp(['Begin cvx iteration : ' num2str(iter)]);
    
    cvx_begin
        variables w(m) x(m) y(m) z(m) dx(n) dy(n) dz(n);
        
        % declare variables used in cvx
        approxP = P + [dx dy dz];
        Z = []; clear C;
        for i=1:n-1
           C(i,:) = [x((i-1)*N + 1) y((i-1)*N + 1) z((i-1)*N + 1)];
           Z = [Z; approxP(i,:); C(i,:)];
        end
        Z = [Z; approxP(n,:)];
       
        % optimize function
        minimize( paramDist*sum(w) + paramCurve*norm(T2*Z,'fro'));

        subject to

            % Cone Form
            for i=1:m
                ind = int8(floor(((i-1)/N)+eps)+1);

                A = A1(i)-A2(i);
                B = B11(i)*approxP(ind,:) + B12(i)*approxP(ind+1,:) - B21(i)*approxP(ind,:) - B22(i)*approxP(ind+1,:);

                norm(A*[x(i) y(i) z(i)] + B) <= w(i);        
            end

            % C1 continuity
            for i=1:(m/N)-1
                ind = (i-1)*N + 1;
                approxP(i+1,:) == (1/2)*([x(ind) y(ind) z(ind)] + [x(ind+N) y(ind+N) z(ind+N)]); 
            end

            % constraint that C1=C2=...=CN
            T*[x y z] == zeros(m,3);  

            % constraint on delta
            if(~isempty(newIndex))
                dz(newIndex) >= zeros(length(newIndex),1);    
%                 abs([dx(newIndex) dy(newIndex)]) <= delta2*ones(length(newIndex),2);
            end
            abs([dx(origIndex) dy(origIndex) dz(origIndex)]) <= delta*ones(length(origIndex),3);
            
            z >= zeros(m,1);
            
            % arcs that should be preserved from previous optimization
            for i=1:length(fixedIndex)
                ind = (fixedIndex(i)-1)*N + 1;
                abs([x(ind) y(ind) z(ind)] - fixedOffPoints(i,:)) <= margin;
            end
            
            
    cvx_end
    cvx_status
    
    if(~strcmp(cvx_status,'Solved')) 
        if (numAddPoints == 1)
            numAddPoints = 2; checkArcs = []; fixedIndex = []; firstRun = 1;
            P = startP; 
            [P origIndex newIndex] = interpolateP(P, sites);
            disp('Trying Optimization again by adding dual points');
            
            continue;
        else
            disp('Optimization not solved for given conditions!');
            break; 
        end
    end
    
    % check results
    if(firstRun ==1 ) checkArcs = 1:n-1; end;
%     checkArcs = 1:n-1;
    [checkArcs newP origIndex newIndex fixedIndex fixedOffPoints] = checkResult(x,y,z,P,approxP,origIndex,sites,triangles, checkArcs, n, N,gridStructure, numAddPoints);
    
    % visualization
    [C Z] = visualizeCurve(x,y,z,P,approxP,n,N,origIndex);
    
    % update P and re-run cvx
    if(length(newP)==length(P) || iter == iteration) break;
    else
        disp([ num2str(length(newP)-length(P)) ' new nodes added']);
        P = newP;
    end
    
end

if(strcmp(cvx_status,'Solved'))
%     calculateStatistics(Z,sites,triangles,gridStructure);
%     save('../data/finalData/hw4.sequence','Z','-ASCII', '-tabs')
end